/*
 * find_marker_server.cpp
 *
 *  Created on: Dec 18, 2012
 *      Author: michiel
 */

#include "servers/find_marker_server.h"

FindMarkerAction::FindMarkerAction(std::string name) : BaseServer(name),
	as_(nh_, name, boost::bind(&FindMarkerAction::execute, this, _1), false)
{
	as_.start();
	markers_sub_ = nh_.subscribe(Topic::AR_POSE, 1, &FindMarkerAction::markerCallback, this);
	_search_id = 0;
	setSearching(false);
}
FindMarkerAction::~FindMarkerAction() {
}

inline void FindMarkerAction::setSearchID(unsigned int id) {
	_search_id = id;
}

inline void FindMarkerAction::setSearching(bool state) {
	_searching = state;
}

inline bool FindMarkerAction::getSearching() {
	return _searching;
}

inline unsigned int FindMarkerAction::getSearchID() {
	return _search_id;
}

inline unsigned int FindMarkerAction::getTotalMarkers() {
	return _total_markers;
}

void FindMarkerAction::markerCallback(const ar_pose::ARMarkers::ConstPtr &msg) {
	_total_markers = 0;
	if (getSearching()) {
		msg->markers.size();
		for (unsigned int i = 0; i < msg->markers.size(); i++) {
			ar_pose::ARMarker marker = msg->markers[i];
			if (marker.id == getSearchID() && marker.pose.pose.position.z < 0.71) {
				_total_markers++;
			}
		}
		ROS_INFO("Total markers: %i", getTotalMarkers());
	}
}

void FindMarkerAction::execute(const rp_states::FindMarkerGoalConstPtr &goal) {
	setSearching(true);
	setSearchID(goal->marker_id);

	ros::Time time = ros::Time::now();
	while (getTotalMarkers() == 0 && getSearching()) {
		geometry_msgs::Twist msg;
		unsigned int time_diff = ros::Time::now().sec - time.sec;

		if (time_diff < 1) {
			msg.angular.z = -0.2;
		} else if (time_diff > 2) {
			msg.angular.z = 0;
			setSearching(false);
		} else {
			msg.angular.z = 0.2;
		}

		// TODO Only turns, use angular message?
		publishWheelsSpeed(msg);
	}

	if (getTotalMarkers() > 0) {
		result_.marker_found = true;
	} else {
		result_.marker_found = false;
	}

	setSearching(false);
	as_.setSucceeded(result_);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "find_marker");

	ROS_INFO("Starting server.");
	FindMarkerAction findmarker(ros::this_node::getName());
	ros::spin();

	return 0;
}
